
#define COLLISION_THRESHOLD 0.06


#include <iostream>
#include <cmath>
#include "CollisionWorldViewer.h"
#include "GlutStuff.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"

//#include <stdio.h> //printf debugging
#include "GLDebugDrawer.h"


static GLDebugDrawer gDebugDraw;


void CollisionWorldViewer::clientMoveAndDisplay()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

	//simple dynamics world doesn't handle fixed-time-stepping
	float ms = getDeltaTimeMicroseconds();
	
	///step the simulation
	if (m_dynamicsWorld)
	{
		m_dynamicsWorld->stepSimulation(ms / 1000000.f);
		//optional but useful: debug drawing
		m_dynamicsWorld->debugDrawWorld();
	}
		
	renderme(); 


	glFlush();

	swapBuffers();

}



void CollisionWorldViewer::displayCallback(void) {

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	
	renderme();

	//optional but useful: debug drawing to detect problems
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();

	glFlush();
	swapBuffers();
}

void	CollisionWorldViewer::initPhysics(){

	CollisionWorldViewer::initPhysics(0,0,0);
	
}


void	CollisionWorldViewer::initPhysics(double targetX, double targetY, double targetZ)
{
	setTexturing(true);
	setShadows(false);
	//setCameraUp(btVector3(btScalar(0),btScalar(30),btScalar(0)));

	//setCameraForwardAxis(0);
	setCameraDistance(5);
	CollisionWorldViewer::m_cameraTargetPosition = btVector3(btScalar(-targetX),btScalar(-targetY),btScalar(targetZ));


	///collision configuration contains default setup for memory, collision setup
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	//m_collisionConfiguration->setConvexConvexMultipointIterations();
	
	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
	
	m_broadphase = new btDbvtBroadphase();
	
	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;

	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
	m_dynamicsWorld->setDebugDrawer(&gDebugDraw);

}

void	CollisionWorldViewer::addGround(double y)
{
	
	///Create ground
	btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(6),btScalar(0.01),btScalar(3)));
	//groundShape->initializePolyhedralFeatures();
//	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
	
	m_collisionShapes.push_back(groundShape);
	 
	btTransform groundTransform;
	groundTransform.setIdentity();
	groundTransform.setOrigin(btVector3(0,y,6.5));

	//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:

	btScalar mass(0.);

	btVector3 localInertia(0,0,0);
		
	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);


	//add the body to the dynamics world
	m_dynamicsWorld->addRigidBody(body);
}

void	CollisionWorldViewer::addDefaultSizedRigidBody(double posX, double posY, double posZ){


	//create a few dynamic rigidbodies
	// Re-using the same collision is better for memory usage and performance

	btBoxShape* colShape = new btBoxShape(btVector3(cubeSize, cubeSize, cubeSize));
	//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
	m_collisionShapes.push_back(colShape);

	/// Create Dynamic Objects
	btTransform startTransform;
	startTransform.setIdentity();

	btScalar	mass(0.f);

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	btVector3 localInertia(0,0,0);
		
	startTransform.setOrigin(btVector3(btScalar(-posX),btScalar(-posY),btScalar(posZ)));
					
	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
	m_dynamicsWorld->addRigidBody(body);

}



void CollisionWorldViewer::addSphere(double posX, double posY, double posZ, double r){

	

	btSphereShape* sphere = new btSphereShape(r);
	
	m_collisionShapes.push_back(sphere);

	/// Create Dynamic Objects
	btTransform startTransform;
	startTransform.setIdentity();

	btScalar	mass(0.f);

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	btVector3 localInertia(0,0,0);
		
	startTransform.setOrigin(btVector3(btScalar(-posX),btScalar(-posY),btScalar(posZ)));
					
	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,sphere,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
	m_dynamicsWorld->addRigidBody(body);
	


}


double CollisionWorldViewer::addHand( double centroidX, double centroidY, double centroidZ, double wristX, double wristY, double wristZ, double graspingPoint1X, double graspingPoint1Y, double graspingPoint1Z,double graspingPoint2X, double graspingPoint2Y, double graspingPoint2Z, double fingerSize, double fingerLength, double graspWidth, double handHeight, double handLength, double teta1, double teta2, double teta3){

	
	//Old implementatio
	double handX, handY, handZ;
	double hip = 5;
	/*double alpha = teta1 + teta3;						//X
	double gamma = 1.5707 + teta2 * sin(alpha);			//Y
	double beta = 1.5707 + teta2;
	double beta = 0;//   teta2 * cos(alpha);			*/		//Z

	double alpha = 1.5707;						//X
	double beta =  -teta2;
	double gamma = teta1 + teta3;			//Y
	

	double xc = wristX;
	double yc = wristY;
	double zc = wristZ;


	std::cout << "\nalpha: " << alpha << "\nbeta: " << beta << "\ngamma: " << gamma << std::endl;


	/*alpha = 0;
	beta = 0;*/


	/*if(alpha > 0)
		alpha = -alpha;*/
	

	std::cout << "\nxc" << xc << "\nyc" << yc << "\nzc" << zc << "\n" << std::endl;
	std::cout << "\nCx: " << centroidX << "\nCy: " << centroidY << "\nCz: " << centroidZ << std::endl;
	std::cout << "\nWx: " << wristX << "\nWy: " << wristY << "\nWz: " << wristZ << std::endl;
	std::cout << "\n\nG1x: " << graspingPoint1X << "\nG1y: " << graspingPoint1Y << "\nG1z: " << graspingPoint1Z << std::endl;
	std::cout << "\n\nG2x: " << graspingPoint2X << "\nG2y: " << graspingPoint2Y << "\nG2z: " << graspingPoint2Z << std::endl;


	/*addSphere(graspingPoint1X, graspingPoint1Y, graspingPoint1Z, 0.05);
	addSphere(graspingPoint2X, graspingPoint2Y, graspingPoint2Z, 0.05);*/

	
	//First finger
	firstFinger = addCollisionBox(	graspingPoint1X - xc*fingerLength,
									graspingPoint1Y + zc*fingerLength,
									graspingPoint1Z - yc*fingerLength,
									fingerSize, 
									fingerLength, 
									fingerSize, 
									-alpha, 
									beta, 
									gamma);

	
	secondFinger = addCollisionBox( graspingPoint2X - xc*fingerLength,
									graspingPoint2Y + zc*fingerLength,
									graspingPoint2Z - yc*fingerLength,
									fingerSize, 
									fingerLength, 
									fingerSize, 
									-alpha, 
									beta, 
									gamma);

	

	
	handX = (graspingPoint1X + graspingPoint2X) / 2;
	handY = (graspingPoint1Y + graspingPoint2Y) / 2;
	handZ = (graspingPoint1Z + graspingPoint2Z) / 2;
	
	hand = addCollisionBox(	handX - xc * (fingerLength*2 + handHeight + COLLISION_THRESHOLD),
							handY + zc * (fingerLength*2 + handHeight + COLLISION_THRESHOLD),
							handZ - yc * (fingerLength*2 + handHeight + COLLISION_THRESHOLD),
							handLength, 
							handHeight, 
							graspWidth + fingerSize*2,
							-alpha,
							beta,
							gamma);
	

	/*//First finger
	firstFinger = addCollisionBox(	fingerPoint1X,
									fingerPoint1Y,
									fingerPoint1Z,
									fingerSize, 
									fingerLength, 
									fingerSize, 
									-alpha, 
									beta, 
									gamma);

	
	secondFinger = addCollisionBox( fingerPoint2X,
									fingerPoint2Y,
									fingerPoint2Z,
									fingerSize, 
									fingerLength, 
									fingerSize, 
									-alpha, 
									beta, 
									gamma);

	

	
	handX = (graspingPoint1X + graspingPoint2X) / 2;
	handY = (graspingPoint1Y + graspingPoint2Y) / 2;
	handZ = (graspingPoint1Z + graspingPoint2Z) / 2;
	
	hand = addCollisionBox(	handX - (fingerLength*2 + handHeight + COLLISION_THRESHOLD) * sin(beta) * cos(gamma),
							handY + (fingerLength*2 + handHeight + COLLISION_THRESHOLD) * cos(beta) * cos(gamma),
							handZ - (fingerLength*2 + handHeight + COLLISION_THRESHOLD) * cos(beta) * sin(gamma),
							handLength, 
							handHeight, 
							graspWidth + fingerSize*2,
							-alpha,
							beta,
							gamma);
							*/
	
	

	return checkCollisions();
	
}


/* Old implementation
float CollisionWorldViewer::addHand( float centroidX, float centroidY, float centroidZ, float graspX, float graspY, float graspZ, float fingerSize, float fingerLength, float graspWidth, float handHeight, float handLength, float beta){
	
	float hipX;
	float alpha;
	
	float handX, handY, handZ;
	
	hipX = sqrt((centroidX - graspX) * (centroidX - graspX) + (centroidZ - graspZ) * (centroidZ - graspZ));
	alpha = acos((centroidZ - graspZ)/hipX);
	

	if(graspX < centroidX)
		alpha = -alpha;

	//First finger
	firstFinger = addCollisionBox(	graspX + (PREDEFINED_GRASP_DISTANCE*sin(alpha)) + (fingerLength * sin(beta))*cos(alpha), 
									graspY - (fingerLength) + (fingerLength - fingerLength * cos(beta)), 
									graspZ - (PREDEFINED_GRASP_DISTANCE*cos(alpha)) + (fingerLength * sin(beta))*sin(alpha), 
									fingerSize, 
									fingerLength, 
									fingerSize, 
									alpha, 
									0, 
									beta);

	

	//Second finger
	secondFinger = addCollisionBox(		graspX - ( graspWidth*2 - (PREDEFINED_GRASP_DISTANCE) )* sin(alpha) + (fingerLength * sin(beta))*cos(alpha), 
										graspY - (fingerLength) + (fingerLength - fingerLength * cos(beta)), 
										graspZ + ( graspWidth*2 - (PREDEFINED_GRASP_DISTANCE))* cos(alpha) + (fingerLength * sin(beta))*sin(alpha), 
										fingerSize, 
										fingerLength, 
										fingerSize, 
										alpha, 
										0, 
										beta);

	
	handX = (graspX + (PREDEFINED_GRASP_DISTANCE*sin(alpha)) + (fingerLength * sin(beta))*cos(alpha) + graspX - ( graspWidth*2 - (PREDEFINED_GRASP_DISTANCE) )* sin(alpha) + (fingerLength * sin(beta))*cos(alpha)) / 2;//  +  ((fingerLength*2 * sin(beta)) * sin(alpha));
	handY = graspY - (2*fingerLength)*cos(beta) - handHeight*cos(beta);
	handZ = (graspZ - (PREDEFINED_GRASP_DISTANCE*cos(alpha)) + (fingerLength * sin(beta))*sin(alpha) + graspZ + ( graspWidth*2 - (PREDEFINED_GRASP_DISTANCE))* cos(alpha))  / 2;//  +  ((fingerLength * cos(beta)) * cos(alpha));

	//Hand
	hand = addCollisionBox(	handX  +  (fingerLength  * sin(beta) * cos(alpha)) + (handHeight * sin(beta) * cos(alpha)) + COLLISION_THRESHOLD*sin(beta)*cos(alpha),
							handY - COLLISION_THRESHOLD * cos(beta),
							handZ  +  (fingerLength*2  * sin(beta) * sin(alpha)) + COLLISION_THRESHOLD * sin(beta) * sin(alpha),
							handLength, 
							handHeight, 
							graspWidth + fingerSize*2,
							alpha,
							0,
							beta);

	return alpha;
}
*/

btRigidBody* CollisionWorldViewer::addCollisionBox(double posX, double posY, double posZ, double sizeX, double sizeY, double sizeZ, double alpha, double beta, double gamma){


	//create a few dynamic rigidbodies
	// Re-using the same collision is better for memory usage and performance

	btBoxShape* colShape = new btBoxShape(btVector3(sizeX, sizeY, sizeZ));
	//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
	m_collisionShapes.push_back(colShape);

	/// Create Dynamic Objects
	btTransform startTransform;
	startTransform.setIdentity();

	btScalar	mass(0.f);

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	btVector3 localInertia(0,0,0);
		
	startTransform.setOrigin(btVector3(btScalar(-posX),btScalar(-posY),btScalar(posZ)));
	
	btQuaternion* quaternion = new btQuaternion;
	btQuaternion* quaternion1 = new btQuaternion;
	btQuaternion* quaternion2 = new btQuaternion;
	btQuaternion* quaternion3 = new btQuaternion;
	
	/*quaternion1->setRotation(btVector3(0,0,0),90);
	quaternion2->setRotation(btVector3(0,1,0),90);
	quaternion3->setRotation(btVector3(0,0,1),90);*/

	//quaternion->setEuler(yaw,pitch,roll);
	//quaternion->setRotation(btVector3(btScalar(1),btScalar(1),btScalar(0)),1.57);

	
	quaternion1->setRotation(btVector3(btScalar(1),btScalar(0),btScalar(0)),alpha);
	quaternion2->setRotation(btVector3(btScalar(0),btScalar(0),btScalar(1)),beta);
	quaternion3->setRotation(btVector3(btScalar(0),btScalar(1),btScalar(0)),gamma);


	*quaternion = *quaternion3 * *quaternion2 * *quaternion1;
	quaternion->setEuler(alpha,beta,gamma);

	//*quaternion = *(quaternion1) * *(quaternion2) * *(quaternion3);
	
	startTransform.setRotation(*(quaternion));


	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
	m_dynamicsWorld->addCollisionObject(body);

	
	return body;
}



	

void CollisionWorldViewer::removeHand(){
	
	m_dynamicsWorld->removeCollisionObject(firstFinger);
	m_dynamicsWorld->removeCollisionObject(secondFinger);
	m_dynamicsWorld->removeCollisionObject(hand);
	firstFinger = NULL;
	secondFinger = NULL;
	hand = NULL;

}


void	CollisionWorldViewer::clientResetScene()
{
	exitPhysics();
	initPhysics();
}
	

int		CollisionWorldViewer::checkCollisions(){

	struct   MyContactResultCallback : public btCollisionWorld::ContactResultCallback
	{
		bool m_connected;
		btScalar m_margin;
		MyContactResultCallback() :m_connected(false),m_margin(0.05){}


		virtual   btScalar   addSingleResult(btManifoldPoint& cp,   const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
		{
			m_connected = true;
			return 1.f;
		}


	};

	MyContactResultCallback callback;
	callback.m_connected = false;

	
	m_dynamicsWorld->contactTest(firstFinger, callback);
	m_dynamicsWorld->contactTest(secondFinger, callback);
	m_dynamicsWorld->contactTest(hand, callback);
	
	if(callback.m_connected)
		return 0;

	return 1;
	

}

void	CollisionWorldViewer::exitPhysics()
{

	//cleanup in the reverse order of creation/initialization

	//remove the rigidbodies from the dynamics world and delete them
	int i;
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}
	m_collisionShapes.clear();

	delete m_dynamicsWorld;
	
	delete m_solver;
	
	delete m_broadphase;
	
	delete m_dispatcher;

	delete m_collisionConfiguration;

	
}